
package com.chrishobson.maths;

import java.lang.Math;

public class Matrix44 {
  public Matrix44() {
  }

  public Matrix44(Matrix44 mat) {
    become(mat);
  }

  /**
    Become an exact copy of the specified matrix
   */
  public void become(Matrix44 mat) {
    for(int i = 0; i < 16; ++i) {
      m_vec[i] = mat.m_vec[i];
    }
  }

  /**
    Sets the matrix to the identify matrix
   */
  public void make_identity() {
    become(m_iden);
  }

  /**
    Sets the matrix to be a transformation by a Point3
   */
  public void make_transform(Point3 p) {
    make_identity();
    m_vec[12] = p.x();
    m_vec[13] = p.y();
    m_vec[14] = p.z();
  }

  public void make_rotation_x(double angle) {
    make_identity();
    double a = Math.toRadians(angle);
    double s = Math.sin(a);
    double c = Math.sqrt(1 - s*s);

    m_vec[5] = c;
    m_vec[6] = -s;
    m_vec[10] = c;
    m_vec[9] = s;
  }

  public void make_rotation_y(double angle) {
    make_identity();
    double a = Math.toRadians(angle);
    double s = Math.sin(a);
    double c = Math.sqrt(1 - s*s);

    m_vec[0] = m_vec[10] = c;
    m_vec[2] = s;
    m_vec[8] = -s;
  }

  /**
    Sets the matrix to be a rotation matrix, around the Z axis by
    a specified number of degrees
   */
  public void make_rotation_z(double angle) {
    make_identity();
    double a = Math.toRadians(angle);
    double s = Math.sin(a);
    double c = Math.sqrt(1 - s*s);

    m_vec[0] = m_vec[5] = c;
    m_vec[1] = -s;
    m_vec[4] = s;
  }

  /**
   * Sets the matix to rotate around the vector by the specified number
   * of degrees.
   * @param scale
   */
  public void make_rotation_vector(Vector3D a_Vec, double angle) {
    double l_I = a_Vec.GetI();
    double l_J = a_Vec.GetJ();
    double l_K = a_Vec.GetK();
    
    // The vector must be unit
    double l_Len = l_I*l_I + l_J*l_J + l_K*l_K ;
    if(l_Len < 0.9999999 || l_Len > 1.0000001) {
      l_Len = Math.sqrt(l_Len);
      l_I /= l_Len;
      l_J /= l_Len;
      l_K /= l_Len;
    }

    double a = Math.toRadians(angle);
    double c = Math.cos(a);
    double s = Math.sin(a);
    double t = 1-c;
    
    m_vec[0] = t * l_I * l_I  + c ;      //tX2 + c
    m_vec[1] = t * l_I * l_J + s * l_K; // tXY + sZ
    m_vec[2] = t * l_I * l_K - s * l_J; // tXZ - sY
    m_vec[3] = 0;

    m_vec[4] = t * l_I * l_J - s * l_K; // tXY-sZ
    m_vec[5] = t * l_J * l_J + c;       // tY2 + c
    m_vec[6] = t * l_J * l_K + s * l_I; // tYZ + sX
    m_vec[7] = 0;

    m_vec[8] = t * l_I * l_J + s * l_J; // tXY + sY
    m_vec[9] = t * l_J * l_K - s * l_I; // tYZ - sX
    m_vec[10] = t * l_K *l_K + c;       // tZ2 + c
    m_vec[11] = 0;

    m_vec[12] = m_vec[13] = m_vec[14] = 0;
    m_vec[15] = 1;
  }
  
  public void make_scale(double scale) {
    make_identity();
    m_vec[0] = m_vec[5] = m_vec[10] = scale;
    

  }



  /**
    Concatenate the Matrix A and B to create a new matrix C
    The matrix C is equivalent to a tranformation by A followed
    by a transformation by B
   */
  public void concat(Matrix44 ma, Matrix44 mb) {
    double [] a = ma.m_vec;
    double [] b = mb.m_vec;
    double [] c = m_vec;

    c[0 ] = a[0 ] * b [0 ] + a[1 ] * b[4 ] + a[2 ] * b[8 ] + a[3 ] * b[12];
    c[1 ] = a[0 ] * b [1 ] + a[1 ] * b[5 ] + a[2 ] * b[9 ] + a[3 ] * b[13];
    c[2 ] = a[0 ] * b [2 ] + a[1 ] * b[6 ] + a[2 ] * b[10] + a[3 ] * b[14];
    c[3 ] = a[0 ] * b [3 ] + a[1 ] * b[7 ] + a[2 ] * b[11] + a[3 ] * b[15];

    c[4 ] = a[4 ] * b [0 ] + a[5 ] * b[4 ] + a[6 ] * b[8 ] + a[7 ] * b[12];
    c[5 ] = a[4 ] * b [1 ] + a[5 ] * b[5 ] + a[6 ] * b[9 ] + a[7 ] * b[13];
    c[6 ] = a[4 ] * b [2 ] + a[5 ] * b[6 ] + a[6 ] * b[10] + a[7 ] * b[14];
    c[7 ] = a[4 ] * b [3 ] + a[5 ] * b[7 ] + a[6 ] * b[11] + a[7 ] * b[15];

    c[8 ] = a[8 ] * b [0 ] + a[9 ] * b[4 ] + a[10] * b[8 ] + a[11] * b[12];
    c[9 ] = a[8 ] * b [1 ] + a[9 ] * b[5 ] + a[10] * b[9 ] + a[11] * b[13];
    c[10] = a[8 ] * b [2 ] + a[9 ] * b[6 ] + a[10] * b[10] + a[11] * b[14];
    c[11] = a[8 ] * b [3 ] + a[9 ] * b[7 ] + a[10] * b[11] + a[11] * b[15];

    c[12] = a[12] * b [0 ] + a[13] * b[4 ] + a[14] * b[8 ] + a[15] * b[12];
    c[13] = a[12] * b [1 ] + a[13] * b[5 ] + a[14] * b[9 ] + a[15] * b[13];
    c[14] = a[12] * b [2 ] + a[13] * b[6 ] + a[14] * b[10] + a[15] * b[14];
    c[15] = a[12] * b [3 ] + a[13] * b[7 ] + a[14] * b[11] + a[15] * b[15];
  }

  /**
   * Concatenate this Matrix with Matrix A
   * 
   * This Matrix becomes equivalent to a transformation by itself followed
   * by a transformation by A
   */
  public void concat(Matrix44 a) {
    // Use the static temporary matrix to store the result
    // of the concatentation of this and A and then swap the
    // arrays over. This avoids allocating any new objects
    m_temp.concat(this,a);
    double [] temp = m_vec;
    m_vec = m_temp.m_vec;
    m_temp.m_vec = temp;
  }


  public void transform(Point3 p) {
    transform(p,p);
  }

  /**
   * Transforms Point3 'in' by the Matrix44 storing result in 'out'
   */
  public void transform(Point3 in, Point3 out) {
    m_d1[0] = in.x();
    m_d1[1] = in.y();
    m_d1[2] = in.z();
    
    transform();

    out.set_x(m_d1[0]);
    out.set_y(m_d1[1]);
    out.set_z(m_d1[2]);
  }

  /**
   * Transforms the xyz (or ijk) by the matrix, the values are
   * placed into m_d1 before calling and are returned in it.
   */
  private void transform() {
    double x = m_d1[0];
    double y = m_d1[1];
    double z = m_d1[2];

    m_d1[0] = x * m_vec[0] + y * m_vec[4] + z * m_vec[8 ] + m_vec[12];
    m_d1[1] = x * m_vec[1] + y * m_vec[5] + z * m_vec[9 ] + m_vec[13];
    m_d1[2] = x * m_vec[2] + y * m_vec[6] + z * m_vec[10] + m_vec[14];
  }

  public void transform(Vector3D in, Vector3D out) {
     m_d1[0] = in.GetI();
     m_d1[1] = in.GetJ();
     m_d1[2] = in.GetK();
     transform();
     out.SetIJK(m_d1[0], m_d1[1], m_d1[2]);
  }
  
  private double m_vec[] = {
    1.0,0.0,0.0,0.0,
    0.0,1.0,0.0,0.0,
    0.0,0.0,1.0,0.0,
    0.0,0.0,0.0,1.0};

  private static Matrix44 m_temp = new Matrix44();
  private static Matrix44 m_iden = new Matrix44();
  
  private static double m_d1[] = new double[3];
}
